use std::sync::{atomic::Ordering, Arc};

use axum::{response::IntoResponse, routing::post, Extension, Json, Router};
use realman::traits::MotionTrait;
use serde::Deserialize;

use crate::{
    server::{ApiOk, AppState, ClampCmd, SensorType},
    Result,
};

pub fn router() -> Router {
    Router::new()
        .route("/", post(post_sensor))
        .route("/speed", post(post_speed))
}

#[derive(Debug, Deserialize)]
struct SpeedData {
    speed: i64,
}
async fn post_speed(
    Extension(app_state): Extension<Arc<AppState>>,
    body: String,
) -> Result<impl IntoResponse> {
    let post_data: SpeedData = serde_json::from_str(&body)?;

    let speed = post_data.speed.clamp(0, 100) as u32;

    app_state.speed.store(speed, Ordering::Relaxed);

    Ok(Json(ApiOk::OK))
}

#[derive(Debug, Deserialize)]
struct SensorData {
    sensor: SensorType, // 类型
    stop: bool,
    speed: i64,
    x: f64,
    y: f64,
    // z: f64,
    // clamp: Option<ClampCmd>, // 夹抓的开和关
}
async fn post_sensor(
    Extension(app_state): Extension<Arc<AppState>>,
    body: String,
) -> Result<impl IntoResponse> {
    let post_data: SensorData = serde_json::from_str(&body)?;

    let speed = post_data.speed.clamp(0, 100) as u32;

    let x = if post_data.x < -3.0 {
        -1.0
    } else if post_data.x > 3.0 {
        1.0
    } else {
        0.0
    };
    let y = if post_data.y < -3.0 {
        -1.0
    } else if post_data.y > 3.0 {
        1.0
    } else {
        0.0
    };
    // 处理数据
    match post_data.sensor {
        SensorType::Car => {
            // 如果是停止
            if post_data.stop {
                app_state.car_listen.send((0.0, 0.0)).ok();
            } else {
                app_state
                    .car_listen
                    .send((-y * (speed as f64 / 100.0), x * (speed as f64 / 100.0)))
                    .ok();
            }
        }
        SensorType::Arm => {
            // 如果是停止
            if let Ok(mut arm) = app_state.arm_listen.lock() {
                if post_data.stop || (x == 0.0 && y == 0.0) {
                    arm.set_arm_slow_stop().ok();
                } else {
                    // arm.set_arm_slow_stop().ok();

                    // arm
                    // x 大于 0 对应 arm 的 y 轴正向; x 小于 0 对应 arm 的 y 轴负向
                    // y 小于 0 对应 arm 的 x 轴正向; y 大于 0 对应 arm 的 x 轴负向
                    let (teach_type, direction_type) = if post_data.x.abs() >= post_data.y.abs() {
                        if x > 0.0 {
                            (realman::TeachType::Y, realman::DirectionType::Pos)
                        } else {
                            (realman::TeachType::Y, realman::DirectionType::Neg)
                        }
                    } else if y > 0.0 {
                        (realman::TeachType::X, realman::DirectionType::Neg)
                    } else {
                        (realman::TeachType::X, realman::DirectionType::Pos)
                    };
                    arm.set_pos_teach(teach_type, direction_type, speed as u8)
                        .ok();
                }
            }
        }
        SensorType::Clamp => {
            if let Ok(mut arm) = app_state.arm_listen.lock() {
                if post_data.stop || (x == 0.0 && y == 0.0) {
                    arm.set_arm_slow_stop().ok();
                } else {
                    // arm.set_arm_slow_stop().ok();

                    // 夹抓旋转 和 上下
                    // x 大于 0 对应 arm 的末端逆时针旋转; x 小于 0 对应 arm 的末端顺时针旋转
                    // y 小于 0 对应 arm z 轴的负向; y 大于 0 对应 arm z 轴的正向
                    if post_data.x.abs() >= post_data.y.abs() {
                        let direction_type = if x > 0.0 {
                            realman::DirectionType::Neg
                        } else {
                            realman::DirectionType::Pos
                        };
                        arm.set_joint_teach(7, direction_type, speed as u8).ok();
                    } else {
                        let (teach_type, direction_type) = if y > 0.0 {
                            (realman::TeachType::Z, realman::DirectionType::Pos)
                        } else {
                            (realman::TeachType::Z, realman::DirectionType::Neg)
                        };
                        arm.set_pos_teach(teach_type, direction_type, speed as u8)
                            .ok();
                    }
                }
            }
        }
    }

    Ok(Json(()))
}
